#include "KinectInteraction.h"

KinectInteraction::KinectInteraction(){


}
using namespace cv;

/** @brief Iniatialize matrices and create processing objects
*   @param
*   @return
*
* Initializes matrices and processing objects.
*
*/
void KinectInteraction::init(){


    //Try to read config file, set default vals if failed
    if (!readConfig()){
        near = 92;
        far = 121;
        samples = 50;
        min_ratio = 0.7;
        max_ratio = 7.4;

    }

    imgProc = new ImageProcessor();
    detector = new PeopleDetector();
    kinect = new KinectInterface();
    gesture = new GestureDetector();


    rgb = Mat(Size(640,480),CV_8UC3,Scalar(0));
    depth = Mat(Size(640,480),CV_16UC1);
    depthScaled = Mat(Size(640,480),CV_8UC1);
    mask = Mat(Size(640,480),CV_8UC1);
    gray = Mat(Size(640,480),CV_8UC1,Scalar(0));

    handPosList = list<float>();
    handHeadRatioList = list<float>();

    filename = "prototype0.yml";
    currentGesture = "idle";



    namedWindow("rgb",CV_WINDOW_AUTOSIZE);
    namedWindow("depth",CV_WINDOW_AUTOSIZE);

    running = false;

}

bool KinectInteraction::readConfig(){
    string file = "config.xml";
    TiXmlDocument doc(file.c_str());

    // load and check if it was successful
    if (doc.LoadFile()) {
        TiXmlElement *root = doc.RootElement();
        TiXmlNode *setting = root->FirstChildElement();
        TiXmlElement *nearEl = setting->FirstChildElement("near");
        TiXmlElement *farEl = setting->FirstChildElement("far");
        TiXmlElement *sampEl = setting->FirstChildElement("samples");
        //TiXmlElement *minEl = setting->FirstChildElement("minratio");
        //TiXmlElement *maxEl = setting->FirstChildElement("person_max_ratio");
        near = atoi(nearEl->FirstChild()->Value());
        far = atoi(farEl->FirstChild()->Value());
        samples = atoi(sampEl->FirstChild()->Value());
        min_ratio = 0.4;//atof(minEl->FirstChild()->Value());
        max_ratio = 7.4;//atof(maxEl->FirstChild()->Value());
        return true;
    }
    return false;

}

/** @brief Retrieve Kinect image and process to find people
*   @param
*   @return
*
* Start retrieval of Kinect images, use depth image as mask
* and detect people in foreground.
*
*/
void KinectInteraction::run(){

    //start retrieving data
    if(!running){
        kinect->run();
        running = true;
    }
    //get frames
    kinect->getRgbFrame(rgb);
    kinect->getDepthFrame(depth);
    depth.convertTo(depthScaled, CV_8UC1, 255.0/2048.0);

    //pre-process data
    imgProc->createMaskFromDepth(mask, depthScaled, near, far);
    imgProc->getChannels(rgb, channels);
    imgProc->getGrayscaleImg(channels, gray);
    imgProc->applyMask(mask, gray);

    //find pepole
    foundPeople = detector->findPeople(gray, mask, depthScaled);

    //add to list
    if(foundPeople.size() != 0){
        //hand position
        handPosList.push_back(foundPeople.at(0).getHandPos().y);
        if(handPosList.size() > samples) handPosList.pop_front();

        float ratio = (float)foundPeople.at(0).getMaxY()/(float)foundPeople.at(0).getHandPos().x;
        handHeadRatioList.push_back(ratio);
        if(handHeadRatioList.size() > samples) handHeadRatioList.pop_front();

        //centre position
        centrePosList.push_back(foundPeople.at(0).getCentrePos());
        if(centrePosList.size() > samples) centrePosList.pop_front();

        foundPeople.at(0).setSpeed(calcSpeed());
        foundPeople.at(0).setGesture(currentGesture);

    }


}

/** @brief Prints people to XML-formatted string
*   @param
*   @return std::string containing people information
*
* Creates XML document and prints found people as elements.
*
*/
std::string KinectInteraction::printPeople(){
    std::string result = "";

    TiXmlDocument doc;
    TiXmlDeclaration * decl = new TiXmlDeclaration( "1.0", "", "" );
    doc.LinkEndChild(decl);

    TiXmlElement *root = new TiXmlElement("People");

    doc.LinkEndChild(root);

    if(running){

        for(unsigned int i=0; i<foundPeople.size();i++){
            foundPeople.at(i).printXml(*root);
        }

    }
   //std::cout << result << std::endl;
    TiXmlPrinter printer;
    doc.Accept(&printer);
    result = printer.CStr();
    return result;

}

/** @brief Draw images in windows
*   @param
*   @return
*
* Draws the rgb and depth images
*
*/
void KinectInteraction::draw(){
    if(running){
        imshow("rgb", gray);
        imshow("depth", mask);
    }
}

/** @brief Stop interaction
*   @param
*   @return
*
* Destroys windows and stops retrieval of images
*
*/
void KinectInteraction::stop(){
    cvDestroyWindow("rgb");
    cvDestroyWindow("depth");
    running = false;
    kinect->stop();
}

/** @brief Stores a gesture prototype
*   @param
*   @return
*
* Stores current hand/head-ratios in prototype vector
*
*/
void KinectInteraction::record(){
    Mat pro;
    if(running){
        while(handHeadRatioList.size()<samples){
            handHeadRatioList.push_back(0.0);
        }
        pro = gesture->assignCaptureVals(handHeadRatioList);
        prototypes.push_back(pro);
    }
    handHeadRatioList.clear();
}

/** @brief Stores a gesture prototype
*   @param
*   @return
*
* Stores current hand/head-ratios in prototype vector
*
*/
void KinectInteraction::record(Mat &pro){
    if(running){
        while(handHeadRatioList.size()<samples){
            handHeadRatioList.push_back(0.0);
        }
        pro = gesture->assignCaptureVals(handHeadRatioList);
    }
    handHeadRatioList.clear();
}

/** @brief Detect current gesture
*   @param
*   @return
*
* Stores current hand/head-ratio and compares
* with prototypes using CCA
*/
void KinectInteraction::detect(){
    vector<float> rho;
    float r;
    int idx;
    if(running){
        while(handHeadRatioList.size()<samples){
            handHeadRatioList.push_back(0.0);
        }
        Mat capture = gesture->assignCaptureVals(handHeadRatioList);
        for(unsigned int i = 0; i< prototypes.size(); i++){
            r = (gesture->cca(capture, prototypes.at(i)));
            rho.push_back(r);
        }
    }

    //Find largest correlation
    vector<float>::const_iterator max = max_element(rho.begin(),rho.end());


    //If largest correlation is above threshold, set gesture to its index
    if(*max > 0.5 && (max - rho.begin()==2)){
        idx = 2;
    }
    else if(*max > 0.2 && (max - rho.begin()==0)){
        idx = 0;
    }
    else if(*max > 0.1 && (max - rho.begin()==1)){
        idx = 1;
    }
    else{
        idx = 99;
    }

    setGesture(idx);
    handHeadRatioList.clear();
}

/** @brief Collect prototypes
*   @param
*   @return
*
* Reads prototypes. TBD: Read from FileStorage
*/
void KinectInteraction::train(){

    //threat
    Mat pro0 = (Mat_<float>(samples,1) <<
        1.16504860, 1.05960262, 1.01694918, 1.04803491, 1.14014256,
        1.13744080, 1.08108103, 1.08352149, 1.22448981, 1.00628936,
        1.00840342, 1.04347825, 1.13207543, 1.12149537, 1.01052630,
        1.07382548, 1.00208771, 1.05494511, 1.11111116, 1.09339404,
        0.97959185, 0.99378884, 1.07142854, 0.98360658, 1.07382548,
        1.12941182, 1.10344827, 1.03671706, 0.97363085, 1.09589040,
        1.02345419, 1.01479912, 1.11627913, 1.11627913, 1.03896105,
        1.01052630, 1.04803491, 1.10854506, 1.03225803, 1.00628936,
        1.05032825, 1.07865167, 1.11368906, 1.04347825, 1.00628936,
        1.07865167, 1.11368906, 1.04347825, 1.00840342, 1.09839821 );

    //come here
    Mat pro1 = (Mat_<float>(samples,1) <<
        1.03004289, 1.02345419, 1.05263162, 1.14014256, 1.22762144,
        1.25984251, 1.26984131, 1.27320957, 1.27320957, 1.25984251,
        1.24352336, 1.18811882, 1.14832532, 1.07865167, 1.04575169,
        1.02345419, 1.02564108, 1.04803491, 1.06666672, 1.13475180,
        1.19700754, 1.18811882, 1.25984251, 1.27659571, 1.26649082,
        1.25984251, 1.24675322, 1.22137403, 1.18811882, 1.14558470,
        1.09589040, 1.04575169, 1.02345419, 1.02127659, 1.04803491,
        1.09589040, 1.15384614, 1.23711336, 1.28686333, 1.28686333,
        1.27999997, 1.27999997, 1.25654447, 1.21827412, 1.18811882,
        1.16788316, 1.15384614, 1.12149537, 1.07142854, 1.05263162 );

    //i kill you
    Mat pro2 = (Mat_<float>(samples,1) <<
        1.14558470, 1.11111116, 1.21827412, 1.22137403, 1.19106698,
        1.09589040, 0.99585062, 1.11111116, 1.15107918, 1.12149537,
        1.08352149, 1.03671706, 1.09339404, 1.08843541, 1.21518981,
        1.15107918, 1.14832532, 1.09339404, 1.10091746, 1.14285719,
        1.30790186, 1.28686333, 1.08843541, 1.13207543, 1.07865167,
        1.01265824, 0.96192384, 1.01694918, 1.08597291, 1.08843541,
        1.08843541, 1.09090912, 1.07623315, 1.08597291, 1.09090912,
        1.08597291, 1.08597291, 1.08843541, 1.06666672, 1.03225803,
        0.99585062, 1.00418413, 1.09090912, 1.08843541, 1.08843541,
        1.09589040, 1.09090912, 1.08843541, 1.03448272, 1.09339404);

    prototypes.push_back(pro0);
    prototypes.push_back(pro1);
    prototypes.push_back(pro2);


}


/** @brief Prints centre
*   @param
*   @return std::string
*
* Prints centre position of first person found
*
*/
std::string KinectInteraction::printCentrePos(){
    stringstream ss;
    string result = "";

    list<Point2i>::iterator it;

    for(it = centrePosList.begin(); it!=centrePosList.end();it++){
        ss << (*it).x << "\n";
    }

    result = ss.str();
    return result;

}

/** @brief A simple speed measure
*   @param
*   @return float
*
* Computes speed, disregarding distance to screen, which results
* in people close to the screen appearing to move faster.
*/
float KinectInteraction::calcSpeed(){

    float result = 0.0;

    int first = centrePosList.back().x;
    int last = centrePosList.front().x;

    result = (float)(last - first)/640.0;

    if(result > -0.04 && result < 0.04) result = 0.0;


    return result;

}

void KinectInteraction::setGesture(int idx){
    switch(idx){
        case 0:
            currentGesture = "threat";
            break;
        case 1:
            currentGesture = "come_here";
            break;
        case 2:
            currentGesture = "i_kill_you";
            break;
        default:
            currentGesture = "idle";
    }
}
